from functools import lru_cache
import sys

from scipy.optimize import minimize

from tma.utils import settings as s
from tma.utils.custom_types import t_line, t_lines, t_point

from tma.utils.tools import *


def cal_track_info_05(
    track_distance: float,
    bl: t_lines,
    cross_line: t_line,
    bl_time_tick: list[int],
    own_pos,
    bl_orientation,
) -> tuple[float, list, tuple[float, float]]:
    """give a track distance, and bearing lines info. find the smallest_error point"""
    _, try_point = evaluate_track_05(
        track_distance,
        bl,
        cross_line,
        bl_time_tick,
        own_pos,
        bl_orientation,
    )
    track_fitness_error_collection = {}
    smallest_error = sys.maxsize
    try_step = -20
    while try_step < 20:
        try_step += 1
        try_point = (try_point[0] + try_step, calculate_y_coordinate(bl[0], try_point[0] + try_step))
        ok, p1, p2 = find_points_on_line_by_distance(try_point, bl[-1], track_distance)
        if not ok:
            continue
        p1_error = cal_track_fitness_metric_05(
            try_point,
            p1,
            bl,
            bl_time_tick,
            own_pos,
            bl_orientation,
        )
        p2_error = cal_track_fitness_metric_05(
            try_point,
            p2,
            bl,
            bl_time_tick,
            own_pos,
            bl_orientation,
        )

        if p1_error <= p2_error:
            track_fitness_error_collection[p1_error] = (try_point, p1)
        else:
            track_fitness_error_collection[p2_error] = (try_point, p2)

        smallest_error = min(p1_error, p2_error, smallest_error)

    if not track_fitness_error_collection:
        raise Exception("track_fitness_error compare error!")

    (x1, y1), (x2, y2) = track_fitness_error_collection[smallest_error]
    return (smallest_error, [x1, y1, x2, y2], cal_line_angle([x1, y1, x2, y2]))


def find_points_on_line_by_distance(p0: t_point, line: t_line, distance: float | int) -> tuple[bool, t_point, t_point]:
    """
    give a point p, a line l and a value v
    d =  perpendicular distance of p to l
    if d < v, return p1 and p2, where p1->p p2->p == d
    """
    x0, y0 = p0
    x1, y1, x2, y2 = line

    d_p2l = cal_perpendicular_distance(p0, line)
    if d_p2l > distance:
        return False, (0, 0), (0, 0)

    if x2 == x1:
        # print('x2 = = x1')
        dy = m.sqrt(distance**2 - (x0 - x1) ** 2)
        return True, (x1, y0 + dy), (x1, y0 - dy)

    else:
        x0, y0 = find_vertical_point(line, p0)
        d_shift = m.sqrt(distance**2 - d_p2l**2)
        slope, _ = get_line_slope_and_intercept(line)

        # dx ** 2 + (slope * dx) ** 2 = d_shift ** 2
        # dx ** 2 =  d_shift ** 2 / (1 + slope ** 2)
        dx = m.sqrt(d_shift**2 / (1 + slope**2))

        x1 = x0 + dx
        y1 = calculate_y_coordinate(line, x1)
        x2 = x0 - dx
        y2 = calculate_y_coordinate(line, x2)

        assert distance - distance_between_points(p0, [x1, y1]) < 1
        assert distance - distance_between_points(p0, [x2, y2]) < 1
        return True, (x1, y1), (x2, y2)


# @lru_cache(10240)
def cal_track_fitness_metric_05(
    head_point: t_point,
    tail_point: t_point,
    bl: t_lines,
    bl_time_tick: list[int],
    own_pos: tuple[t_point],
    bl_orientation: list[float],
) -> float:
    """calculate track fit metric using bearing angle-diff method"""
    assume_track_line = list(head_point) + list(tail_point)
    points = divide_track_by_bl(assume_track_line, bl_time_tick)
    assert len(points) == len(bl)
    total_error: list[Union[float, int]] = [0]
    for p1, p2, bearing in zip(own_pos, points, bl_orientation):
        # print(p1, p2, bearing)
        br = get_orientation_between_points(p1, p2)
        total_error.append(abs(br - bearing))

    assert len(total_error) - len(own_pos) == 1
    return sum(total_error)


def evaluate_track_05(
    track_distance: float,
    bl: t_lines,
    cross_line: t_line,
    bl_time_tick: list[int],
    own_pos,
    bl_orientation,
) -> tuple[float, t_point]:
    """give track distance, and bearing lines info, calculate track error uing minimize()"""
    outer_lines_cross_p = find_intersection_point(bl[0], bl[-1])
    if not outer_lines_cross_p:
        raise Exception("First and last bearring seems parallel")
    try_point = outer_lines_cross_p
    # point on x-axis where line pass it and view_center is vertical to first_line
    x_value, _ = find_intersection_point(cross_line, bl[0])

    if x_value < outer_lines_cross_p[0]:
        try_step = -s.Try_point_step
    elif x_value > outer_lines_cross_p[0]:
        try_step = s.Try_point_step
    else:
        # @TODO first_line is vertical to x-axis
        raise Exception("predict_target_track error: first_line is vertical to x-axis")

    try_x_bottom = try_point[0] + try_step
    while cal_perpendicular_distance(try_point, bl[-1]) <= track_distance:
        try_point = (
            try_point[0] + try_step,
            calculate_y_coordinate(bl[0], try_point[0] + try_step),
        )
    try_x_top = try_point[0] - try_step

    bounds = [(min(try_x_bottom, try_x_top), max(try_x_bottom, try_x_top)), (None, None)]
    res = minimize(
        cal_try_point_error_05,
        try_point,
        args=(bl, track_distance, bl_time_tick, own_pos, bl_orientation),
        bounds=bounds,
    )

    # print("1" * 55)
    # print(res.success)
    # print(res.x)
    # print(res.fun)
    if res.success:
        return res.fun, res.x
    return evaluate_track_partial_brute_force_05(
        bl,
        track_distance,
        cross_line,
        bl_time_tick,
        own_pos,
        bl_orientation,
    )


def cal_try_point_error_05(
    try_point: t_point,
    lines_used: t_lines,
    travel_distance_used: float,
    bl_time_tick: list[int],
    own_pos,
    bl_orientation,
) -> float:
    ok, p1, p2 = find_points_on_line_by_distance(try_point, lines_used[-1], travel_distance_used)
    if not ok:
        # raise Exception("cal_try_point_error() error!")
        return 99999

    p1_error = cal_track_fitness_metric_05(try_point, p1, lines_used, bl_time_tick, own_pos, bl_orientation)
    p2_error = cal_track_fitness_metric_05(try_point, p2, lines_used, bl_time_tick, own_pos, bl_orientation)

    return min(p1_error, p2_error)


def evaluate_track_partial_brute_force_05(
    bl: t_lines,
    track_distance: float,
    cross_line: t_line,
    bl_time_tick: list[int],
    own_pos,
    bl_orientation,
) -> tuple[float, t_point]:
    track_fitness_error_collection = {}
    outer_lines_cross_p = find_intersection_point(bl[0], bl[-1])
    if not outer_lines_cross_p:
        raise Exception("First and last bearring seems parallel")
    try_point = outer_lines_cross_p
    # point on x-axis where line pass it and view_center is vertical to first_line
    x_value, _ = find_intersection_point(cross_line, bl[0])

    if x_value < outer_lines_cross_p[0]:
        try_step = -s.Try_point_step
    elif x_value > outer_lines_cross_p[0]:
        try_step = s.Try_point_step
    else:
        # @TODO first_line is vertical to x-axis
        raise Exception("predict_target_track error: first_line is vertical to x-axis")

    smallest_error = sys.maxsize
    while cal_perpendicular_distance(try_point, bl[-1]) <= track_distance:
        try_point = (
            try_point[0] + try_step,
            calculate_y_coordinate(bl[0], try_point[0] + try_step),
        )
        ok, p1, p2 = cached_find_points_on_line_by_distance_05(try_point, bl[-1], track_distance)
        if not ok:
            break
        p1_error = cal_track_fitness_metric_05(
            try_point,
            p1,
            bl,
            bl_time_tick,
            own_pos,
            bl_orientation,
        )
        p2_error = cal_track_fitness_metric_05(
            try_point,
            p2,
            bl,
            bl_time_tick,
            own_pos,
            bl_orientation,
        )
        if min(p1_error, p2_error) > smallest_error * 20:
            break

        if p1_error <= p2_error:
            track_fitness_error_collection[p1_error] = (try_point, p1)
        else:
            track_fitness_error_collection[p2_error] = (try_point, p2)

        smallest_error = min(p1_error, p2_error, smallest_error)

    return smallest_error, track_fitness_error_collection[smallest_error][0]


def cached_find_points_on_line_by_distance_05(
    p0: t_point, line: t_line, distance: float | int
) -> tuple[bool, t_point, t_point]:
    """
    give a point p, a line l and a value v
    d =  perpendicular distance of p to l
    if d < v, return p1 and p2, where p1->p p2->p == d
    """
    x0, y0 = p0
    x1, y1, x2, y2 = line

    d_p2l = cal_perpendicular_distance(p0, line)
    if d_p2l > distance:
        return False, (0, 0), (0, 0)

    if x2 == x1:
        # print('x2 = = x1')
        dy = m.sqrt(distance**2 - (x0 - x1) ** 2)
        return True, (x1, y0 + dy), (x1, y0 - dy)

    else:
        x0, y0 = find_vertical_point(line, p0)
        d_shift = m.sqrt(distance**2 - d_p2l**2)
        slope, _ = get_line_slope_and_intercept(line)

        # dx ** 2 + (slope * dx) ** 2 = d_shift ** 2
        # dx ** 2 =  d_shift ** 2 / (1 + slope ** 2)
        dx = m.sqrt(d_shift**2 / (1 + slope**2))

        x1 = x0 + dx
        y1 = calculate_y_coordinate(line, x1)
        x2 = x0 - dx
        y2 = calculate_y_coordinate(line, x2)

        assert distance - distance_between_points(p0, [x1, y1]) < 1
        assert distance - distance_between_points(p0, [x2, y2]) < 1
        return True, (x1, y1), (x2, y2)
